# just for test
import sys
sys.path.append("E:\work\python_work")

import math
from pyrobot.bean.map.point import Point

class PolyLine:
     def __init__(self, threshold, pointList):
         self.pointList = pointList
         self.threshold = threshold
     
     def __leastsquare__(self, data):
        x = []
        y = []
        for coord in data:
            x.append(coord[0])
            y.append(coord[1])
        xsum = sum(x)
        ysum = sum(y)
        xysum = 0
        for (i,j) in data:
            xysum = xysum + i*j
        x2sum = 0
        for (i,j) in data:
            x2sum = x2sum + i**2
        b = (xysum*xsum-ysum*x2sum)/(xsum**2-len(x)*x2sum)
        a = (xsum*ysum-len(x)*xysum)/(xsum**2-len(x)*x2sum)
        
        print "a ", a
        
        return [a,b]
     
     """
     return points sets for each poly line
     """
     def getPolylineSets(self):
         # group points
         currentTan = startTan = None
         startAngle = currentAngle = None
         pointSet = []
         pointGroups = []
         polylineSet = []
         
         firstPoint = self.pointList[0]
         secondPoint = self.pointList[1]
         for i in range(self.pointList.__len__()):
             if i == 0 or i == 1:
                 pointSet.append(pointList[i])
                 continue
             currentAngle = firstPoint.angle(secondPoint, pointList[i]) 
             if currentAngle < self.threshold:
                 pointSet.append(pointList[i])
             else:
                 # group the current points and start a new set
                 pointGroups.append(pointSet)
                 # reset 
                 pointSet = []
                 firstPoint = pointList[i]
                 pointSet.append(firstPoint)
                 if i+1 < self.pointList.__len__():
                     secondPoint = pointList[i+1]
                     pointSet.append(secondPoint)
         if pointSet.__len__() != 0:
             pointGroups.append(pointSet)
        
         # fit polyline in each group
         self.cordset = []
         
         for polyline in pointGroups:
             data = []
             for point in polyline:
                 data.append((point.x,point.y))  
             a, b = self.__leastsquare__(data)   
             # transform to Point type and put it in the set
             pointSet = []
             for coord in data:
                 point = Point(coord[0], a*coord[0]+b)
                 pointSet.append(point)
                 self.cordset.append((coord[0], a*coord[0]+b))
             polylineSet.append(pointSet)
             
         return polylineSet             

         
if __name__=="__main__":
    
#    print sys.path
#    sys.path.append("E:\work\python_work\pyrobot")
    
    print math.atan(1)*180/math.pi
    
    list = [(0.10000000000014558, 2377.5352439975527), (410.92941881717167, 2329.8934190382479), (821.48172609754397, 2256.685421786301), (1314.0864790648402, 2275.8741178801988), (2023.2076706477483, 2411.0366593400981), (2394.1573838231379, 2008.8382690747312), (2360.9063368787756, 1362.9900053178862), (2347.1374149211761, 854.23826146111912), (2312.609938409068, 407.75185442540067), (2200.9946623760538, 0.0), (2282.6089973879907, -402.46187907674141), (2258.4256694287301, -821.94982667206989), (2381.6604450924756, -1374.9723952818567), (2265.1740896305623, -1900.6084344897104), (1936.2865349512672, -2307.4480836014477), (1365.4798828005412, -2364.8901043243327), (824.03038374521805, -2263.6878011232061), (401.16840201860168, -2274.5359419275346), (0.10000000000014347, -2343.2868996795896)]
#    list = [(0.10000000000014558, 2377.5352439975527), (410.92941881717167, 2329.8934190382479), (821.48172609754397, 2256.685421786301), (1314.0864790648402, 2275.8741178801988), (2023.2076706477483, 2411.0366593400981), (2394.1573838231379, 2008.8382690747312), (2360.9063368787756, 1362.9900053178862)]
    
#    list = [(2347.1374149211761, 854.23826146111912), (2312.609938409068, 407.75185442540067), (2200.9946623760538, 0.0), (2282.6089973879907, -402.46187907674141), (2258.4256694287301, -821.94982667206989), (2381.6604450924756, -1374.9723952818567), (2265.1740896305623, -1900.6084344897104), (1936.2865349512672, -2307.4480836014477), (1365.4798828005412, -2364.8901043243327), (824.03038374521805, -2263.6878011232061), (401.16840201860168, -2274.5359419275346), (0.10000000000014347, -2343.2868996795896)]
    
#    list = [(0.10000000000014558, 2377.5352439975527), (410.92941881717167, 2329.8934190382479), (821.48172609754397, 2256.685421786301), (1314.0864790648402, 2275.8741178801988), (2023.2076706477483, 2411.0366593400981)]
    
    pointList = []
    for data in list:
        point = Point(data[0], data[1])
        pointList.append(point)
    
    print pointList.__len__()
    
    poly = PolyLine(10, pointList)
    polylineSet = poly.getPolylineSets()
    for i in range(polylineSet.__len__()):
        print polylineSet[i]
    
    print poly.cordset
#    print polylineSet
    
    
    